//  定义控制台应用程序的入口点。
//

#include<Windows.h>
#include<stdio.h>
#include<string.h>
typedef struct PID_
{
    float actualValue;     //实际值 每次反馈回来的值，模拟传感器采集到的值 
    float error;           //偏差值
    float Kd, Ki, Kp;      //比例、积分、微分常数
    float P, I, D;         //比例项 积分项 微分项
    float errorPre;        //E[k-1]  //上一次误差
    float errorPrePre;     //E[k-2]  //上上次误差
    float setValue;        //设定值 你期望系统达到的值
    float integral;        //积分值
}PID;

/*pid初始化*/
void pid_init(double kp, double ki, double kd, PID *pid)
{
	memset(pid,0,sizeof(PID)); 
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
}

/*位置式pid
@param pid  要计算的pid
@param setValue 想要设定的值
返回 增量
*/
float positionPid(PID *pid)
{ 
	static char sta=0;
	if(!sta){
		sta=~sta;
		printf("位置式PID:\n");
		printf("ID\t输出值\t实际值\t本次误差\tP\tI\tD\n"); 
	}    
    pid->error = pid->setValue - pid->actualValue;
    pid->integral += pid->error;   		/*误差累积 与增量式pid不同，位置式需要进行错误累计积分*/
    pid->P = pid->Kp*pid->error;		//比例项
    pid->I = pid->Ki*pid->integral;		//积分项
    pid->D =pid->Kd*(pid->error-pid->errorPre);//微分项
    pid->errorPrePre = pid->errorPre;
    pid->errorPre = pid->error;
    pid->actualValue = pid->P + pid->I +pid->D;
    return pid->actualValue;

}

/*增量式PID算法
@param pid  要计算的pid
@param setValue 想要设定的值 可以作为入参
返回给执行器的输出值

*/
float incrementalPid(PID *pid)
{
	static char sta=0;
	if(!sta){
		sta=~sta;
		printf("增量式PID:\n");
		printf("ID\t输出值\t传感器值\t本次误差\tP\tI\tD\n"); 
	}
    float increment;
    pid->error = pid->setValue - pid->actualValue;
    pid->P = pid->Kp*(pid->error-pid->errorPre);			//比例项
    pid->I = pid->Ki*pid->error;							//积分项
    pid->D = pid->Kd*(pid->error-2*pid->errorPre + pid->errorPrePre); 			//微分项
    increment = pid->P + pid->I + pid->D;   				//pid计算得到的输出增量
    pid->actualValue += increment;    	 					//实际反馈值加上增量
    pid->errorPrePre = pid->errorPre;
    pid->errorPre = pid->error;
    return increment;
}

int main()
{
    PID pid;
    pid_init(0.7, 0.1, 0.05, &pid);   
    pid.setValue = 500;		//设定目标值为50
    pid.actualValue = 5;   //设定传感器值90 
    //模拟传感器采集到的值  不是实际值 
    for (int i = 0; i <=200; i++)
    {   
        /*特别说明：这里只是单纯的算法实现，并没有实际的设备反馈回来的实际值
        是能说是模拟，具体还的看设备的反馈值
        实际情况下每执行一次positionPid（） actualValue 应该都要变化 
        */
        //float uk = positionPid(&pid);		//位置式输出值 
        float uk = incrementalPid(&pid);	    //增量式输出值 
        printf("%d\t%.2f\t%.2f\t\t%.2f\t\t%.2f\t%.2f\t%.2f\t\n",i,uk,pid.actualValue,pid.error,pid.P, pid.I, pid.D);	
		//可以看到本次误差值在收敛 
    }
    system("pause");
    return 0;
}
